﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;

using System.Diagnostics;

namespace Drifting.MotionPlanning.Systems
{
    public class BiPedalGait : BaseSystem
    {

        public double standing_foot_x;
        public double standing_foot_y;
        public bool standing_foot_right;


        double a;
        double b;
        double m;
        double mH;
        double g;
        public double phi;
        double[,] S;

        public BiPedalGait()
            : base("../../../MotionPlanning/Resources/Drift12D.kdl")
        {
            standing_foot_x = 0;
            standing_foot_y = 0;
            standing_foot_right = true;

            t_delta = 0.001;
            //  model parameters
            g = 9.80665;//m/s/s
            m = 5;      //kg
            mH = 10;    //kg
            a = 0.5;    //m
            b = 0.5;    //m
            phi = 0 / 180.0 * Math.PI;

            this.dof_ctrl = 3;
            this.dof_state = 4;
            this.dof_alias = 4;

            this.max_state = new double[4];
            max_state[0] = 2 * Math.PI;
            max_state[1] = 2 * Math.PI;
            max_state[2] = Math.PI;
            max_state[3] = Math.PI;
            this.min_state = new double[4];
            min_state[0] = Math.PI;
            min_state[1] = Math.PI;
            min_state[2] = -Math.PI;
            min_state[3] = -Math.PI;

            S = new double[2, 3];
            S[0, 0] = 1;
            S[0, 1] = -1;
            S[0, 2] = 0;
            S[1, 0] = 0;
            S[1, 1] = 1;
            S[1, 2] = 1;

            Debug.AutoFlush = true;
        }

        public double A
        { get { return a; } }
        public double B
        { get { return b; } }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            //u_ns = ctrl[0];
            //u_H = ctrl[1];
            //u_s = ctrl[2];

            double theta_ns = q[0];
            double theta_s = q[1];
            double dTheta_ns = q[2];
            double dTheta_s = q[3];
            double l = a + b;

            double[,] M = new double[2, 2];
            M[0, 0] = m * b * b;
            M[0, 1] = -m * l * b * Math.Cos(theta_s - theta_ns);
            M[1, 0] = M[0, 1];
            M[1, 1] = mH * l * l + m * (l * l + a * a);

            double[,] N = new double[2, 2];
            N[0, 0] = 0;
            N[0, 1] = m * l * b * dTheta_s * Math.Sin(theta_s - theta_ns);
            N[1, 0] = -m * l * b * dTheta_ns * Math.Sin(theta_s - theta_ns);
            N[1, 1] = 0;

            double[,] G = new double[2, 1];
            G[0, 0] = m * b * Math.Sin(theta_ns) * g;
            G[1, 0] = -(mH * l + m * a + m * l) * Math.Sin(theta_s) * g;



            // ddTheta = M^{-1} * [Su - G - N * dTheta]

            double[,] M_inv = new double[2, 2];
            if (MathAssistant.Matrix_Inverse_2x2(M, ref M_inv) == false)
                throw new Exception("inverse failure");

            double UGN_0 = -(N[0, 0] * dTheta_ns + N[0, 1] * dTheta_s) - G[0, 0];
            double UGN_1 = -(N[1, 0] * dTheta_ns + N[1, 1] * dTheta_s) - G[1, 0];

            qd[0] = dTheta_ns;
            qd[1] = dTheta_s;
            qd[2] = M_inv[0, 0] * UGN_0 + M_inv[0, 1] * UGN_1;
            qd[3] = M_inv[1, 0] * UGN_0 + M_inv[1, 1] * UGN_1;

        }

        override public void Integration(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {

            Qd(q, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MathAssistant.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MathAssistant.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MathAssistant.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MathAssistant.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MathAssistant.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);

            MathAssistant.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);

            double theta_A = q_next[0];
            double theta_B = q_next[1];
            double dTheta_A = q_next[2];
            double dTheta_B = q_next[3];

            double shouldBeZero = theta_A + theta_B - Math.PI - Math.PI - Math.PI + phi + phi - Math.PI;

            if (Math.Abs(shouldBeZero) < 0.001 && theta_A > theta_B && dTheta_A < 0)
            {
                double cos_Alpha = Math.Cos(theta_A - theta_B);
                double d_Alpha = dTheta_A - dTheta_B;
                double cos_B = Math.Cos(theta_B);
                double sin_B = Math.Sin(theta_B);
                double cos_A = Math.Cos(theta_A);
                double sin_A = Math.Sin(theta_A);
                double l = a + b;

                    double[,] Q_minus = new double[2, 2];
                    Q_minus[0, 0] = -m * a * b;
                    Q_minus[0, 1] = -m * a * b + (mH * l * l + 2 * m * a * l) * cos_Alpha;
                    Q_minus[1, 0] = 0;
                    Q_minus[1, 1] = -m * a * b;

                    double[,] Q_plus = new double[2, 2];
                    Q_plus[0, 0] = m * b * (b - l * cos_Alpha);
                    Q_plus[0, 1] = m * l * (l - b * cos_Alpha) + m * a * a + mH * l * l;
                    Q_plus[1, 0] = m * b * b;
                    Q_plus[1, 1] = -m * b * l * cos_Alpha;

                    double[,] Q_plus_inv = new double[2, 2];
                    if (MathAssistant.Matrix_Inverse_2x2(Q_plus, ref Q_plus_inv) == false)
                        throw new Exception("inverse failure");


                    double[,] H = new double[2, 2];
                    MathAssistant.Matrix_X_(Q_plus_inv, Q_minus, ref H);

                    dTheta_A = H[0, 0] * dTheta_A + H[0, 1] * dTheta_B;
                    dTheta_B = H[1, 0] * dTheta_A + H[1, 1] * dTheta_B;


                q_next[0] = theta_B;         // this is always the swing leg
                q_next[1] = theta_A;         // this is always the support leg
                q_next[2] = dTheta_A;
                q_next[3] = dTheta_B;

                double dx = -Math.Sin(theta_B) * l + Math.Sin(theta_A) * l;
                double dy = Math.Cos(theta_B) * l - Math.Cos(theta_A) * l;

                double new_foot_x = standing_foot_x + dx;
                double new_foot_y = standing_foot_y + dy;

                standing_foot_x = new_foot_x;
                standing_foot_y = new_foot_y;
                standing_foot_right = !standing_foot_right;
            }




        }
    }
}
